#include "vision.h"
#include "usart.h"
#include "fkine.h"
#include "pose.h"

#define vision_lens 26
uint8_t VISION_BUF[vision_lens];
extern pose_t Pose_target;
uint8_t IS_get_vision;

extern DMA_HandleTypeDef hdma_uart8_rx;

void UART8_IRQHandler(void)
{
    uint8_t Last_lens;
    if(__HAL_UART_GET_FLAG(&huart8,UART_FLAG_IDLE)==SET){
        __HAL_UART_CLEAR_IDLEFLAG(&huart8);  //清除标志位，准备下次接收数据
        HAL_UART_DMAStop(&huart8);										//停止dma传输
        //读取ndtr寄存器中dma已发送数据字节数，然后算出距离vision_lens字节还需要多少字节
        Last_lens=vision_lens-hdma_uart8_rx.Instance->NDTR;
        HAL_UART_Transmit_DMA(&huart8,VISION_BUF,Last_lens); //将剩余字节发送到接收数组中
        //在此插入数据处理函数
        IS_get_vision=Vision_decode(VISION_BUF,&Pose_target);
    }
    HAL_UART_Receive_DMA(&huart8,VISION_BUF,vision_lens);		//重启DMA
}

void vision_init(UART_HandleTypeDef *huart)
{
    __HAL_UART_ENABLE_IT(huart,UART_IT_IDLE);	        //开启空闲中断
    HAL_UART_Receive_DMA(huart,VISION_BUF,vision_lens);	//开启DMA并指定接收数组
}

uint8_t char_2_hex(uint8_t *src)
{
    uint8_t desc;
    if((*src >= '0') && (*src <= '9')){
        desc = *src - 0x30;
    }
    else if((*src >= 'a') && (*src <= 'f')){
        desc = *src - 0x57;
    }
    else if((*src >= 'A') && (*src <= 'F')){
        desc = *src - 0x37;
    } else{
        desc=0;
    }
    return desc;
}

uint8_t Vision_decode(uint8_t *pdata,pose_t *robot)
{
    uint8_t IS;
    int temp[6];
    uint8_t char_temp[3];

    if(pdata[0]=='E'&&pdata[25]=='D'){
        for(uint8_t di=0;di<6;di++){
            for(uint8_t ci=0;ci<3;ci++){
                char_temp[ci]= char_2_hex(&pdata[(2+4*di+ci)]);
            }
            temp[di]=char_temp[0]*16*16+char_temp[1]*16+char_temp[2];
            if(pdata[1+di*4]=='-'){
                temp[di]*=-1;
            }
        }
        robot->X=temp[0];
        robot->Y=temp[1];
        robot->Z=temp[2];
        robot->Roll=(float)temp[3];
        robot->Yaw=(float)temp[4];
        robot->Pitch=(float)temp[5];


        IS=1;
    } else{
        IS=0;
    }

    return IS;
}
